//#include <sys/types.h>
//#include <sys/stat.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <unistd.h>
//#include <strings.h>
#include <stdlib.h>

//#include <pthread.h>
//#include <semaphore.h>


#define BAUD_RATE B57600  /* the baud rate */
#define MODEMDEVICE "/dev/ttyS0" /* port for serial port */
//#define MODEMDEVICE "/dev/rfcomm1" /* port for bluetooth connection */

int robot_init(char *port);
void robot_forward();
void robot_backward();
void robot_right();
void robot_left();
void robot_stop();
void robot_bumpers(unsigned char *data);
int robot_close();
static int serial;
int main(int argc, char **argv)
{
//	char* MODEMDEVICE = argv[1];
	printf("ROBOT DEMO\n");

//	int c, res;
//	char buf[6], buf2[6]; /* buffers used for read and write to irobot */
//	char key;


//	printf("About to open modem device\n");
	/* Configure Serial port */
	/* Open modem device for reading and writing */
	printf("about to open\n");
	func();
	if(robot_init(MODEMDEVICE) < 0){
		fprintf(stderr, "could not connect to the robot\n");
		exit(1);
	}
	printf("opened\n");


	/* now clean the modem line and activate the settings for the port */
	//tcflush(fd, TCIFLUSH);
	//tcsetattr(fd,TCSANOW,&config);
//	pthread_mutex_lock(&sensor_mutex);
	//start_buf[0]=128;
	//start_buf[1]=132;
//	sens_buf[0]=142;
//	sens_buf[1]=7;
	//printf("about to write\n");
	//int write_val = write(fd,start_buf,2);
	//printf("write returned %d\n",write_val);
	char c;
	char bumper;
	while(1){
		c = getc(stdin);
		if (c=='c'){
		//	robot_close();
			return(0);
		}
		else if (c == 'f'){
			robot_forward();
		}
		else if (c == 's'){
			robot_stop();
		}
		else if (c == 'r'){
			robot_right();
		}
		else if (c == 'l'){
			robot_left();
		}
		else if (c == 'b'){
			robot_backward();
		}
		else if (c == 't'){
			robot_bumpers(&bumper);
			printf("bumpers: ");
			printf("left=%s  ",((bumper & 2) == 2) ? "yes" : "no");
			printf("right=%s  ",((bumper & 1)== 1) ? "yes" : "no");
			printf("\n");
		}
	}
	return 0;
}

int func(){
	//I don't know why, but this function is necessary
	printf("mission critical print statement\n");
	return 0;
}
int robot_init(char *port) {
	printf("starting robot_init\n");
	struct termios config;
	unsigned char full_mode[] = {128, 132};

	serial = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
	if(serial < 0) {
		return -1;
	}
	printf("just called open\n");
	config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
	//config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
	config.c_oflag = 0;
	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
	config.c_cflag &= ~(CSIZE | PARENB);
	config.c_cflag |= CS8;
	config.c_cc[VMIN]  = 1;
	config.c_cc[VTIME] = 0;
	if (cfsetispeed(&config, BAUD_RATE) < 0 || cfsetospeed(&config, BAUD_RATE) < 0) {
		return -2;
	}
	if(tcsetattr(serial, TCSAFLUSH, &config) < 0) {
		return -3;
	}

	write(serial, full_mode, 2);
	printf("done with robot_init\n");
	return 0;
}

void robot_forward() {
	unsigned char forward[] = {145, 0, 100, 0, 100};
	write(serial, forward, 5);
}

void robot_backward() {
	unsigned char backward[] = {145, 255, 156, 255, 156};
	write(serial, backward, 5);
}

void robot_left() {
	unsigned char left[] = {145, 0, 100, 255, 156};
	write(serial, left, 5);
}

void robot_right() {
	unsigned char right[] = {145, 255, 156, 0, 100};
	write(serial, right, 5);
}

void robot_stop() {
	unsigned char stop[] = {145, 0, 0, 0, 0};
	write(serial, stop, 5);
}

void robot_bumpers(unsigned char *data) {
	unsigned char bump[] = {142, 7};
	write(serial, bump, 2);
	//usleep(100000);
	if(read(serial, data, 1) == -1){
		printf("read returned -1\n");
	}
	(*data) &= 3;
}

int robot_close() {
	return close(serial);
}	
